#include <thread>
#include <iostream>
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <fstream>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ctime>

using namespace std;
#define PI 3.14159
#define DEG2RAD(x) ((x)/180.0*PI)
#define RAD2DEG(x) ((x)/PI*180.0)

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;



int target=0;




ros::Time last_cmd_vel_time;
ros::Duration timeout(1.0);  // 超时时间为2秒




struct Goal 
{
    double x, y, orientation_z, orientation_w;
};

vector<Goal> readGoalsFromFile(const string& filename) 
{
    vector<Goal> goals;
    ifstream file(filename);
    if (file.is_open()) {
        Goal goal;
        while (file >> goal.x >> goal.y >> goal.orientation_z >> goal.orientation_w) {
            goals.push_back(goal);
        }
        file.close();
    }
    return goals;
}

void turn_left_45() //1 -> 18
{
    ROS_INFO("turning left");
    // 使用全局命名空间创建Publisher，不需要显式创建节点句柄
    ros::Publisher cmd_vel_pub = ros::NodeHandle().advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    // 设置发布频率
    ros::Rate rate(10); // 10Hz

    // 创建Twist消息实例
    geometry_msgs::Twist twist;

    // 设置旋转速度 (绕z轴)
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 2.0; // 旋转速度为1弧度/秒

    ROS_INFO("Rotating the robot");

    for(int count=7;count>0;count--) 
    {
        // 发布消息
        cmd_vel_pub.publish(twist);
        // 休眠直到下一个发布周期
        rate.sleep();
    }
}

void turn_left_60() //1 -> 18
{
    ROS_INFO("turning left");
    // 使用全局命名空间创建Publisher，不需要显式创建节点句柄
    ros::Publisher cmd_vel_pub = ros::NodeHandle().advertise<geometry_msgs::Twist>("/cmd_vel", 10);
    // 设置发布频率
    ros::Rate rate(10); // 10Hz

    // 创建Twist消息实例
    geometry_msgs::Twist twist;

    // 设置旋转速度 (绕z轴)
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 2.0; // 旋转速度为1弧度/秒

    ROS_INFO("Rotating the robot");

    for(int count=9;count>0;count--) 
    {
        // 发布消息
        cmd_vel_pub.publish(twist);
        // 休眠直到下一个发布周期
        rate.sleep();
    }
}

void turn_left_90(void) //1 -> 20
{
    ROS_INFO("turning left 90");
    // 使用全局命名空间创建Publisher，不需要显式创建节点句柄
    ros::Publisher cmd_vel_pub = ros::NodeHandle().advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    // 设置发布频率
    ros::Rate rate(10); // 10Hz

    // 创建Twist消息实例
    geometry_msgs::Twist twist;

    // 设置旋转速度 (绕z轴)
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = 2.2; // 旋转速度为1弧度/秒

    ROS_INFO("Rotating the robot");

    for(int count=11;count>0;count--) 
    {
        // 发布消息
        cmd_vel_pub.publish(twist);
        // 休眠直到下一个发布周期
        rate.sleep();
    }
}

void turn_right_90(void) 
{
    ROS_INFO("turning left 90");
    // 使用全局命名空间创建Publisher，不需要显式创建节点句柄
    ros::Publisher cmd_vel_pub = ros::NodeHandle().advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    // 设置发布频率
    ros::Rate rate(10); // 10Hz

    // 创建Twist消息实例
    geometry_msgs::Twist twist;

    // 设置旋转速度 (绕z轴)
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = -2.2; // 旋转速度为1弧度/秒

    ROS_INFO("Rotating the robot");

    for(int count=11;count>0;count--) 
    {
        // 发布消息
        cmd_vel_pub.publish(twist);
        // 休眠直到下一个发布周期
        rate.sleep();
    }
}


void turn_right_60(void) 
{
    // 使用全局命名空间创建Publisher，不需要显式创建节点句柄
    ROS_INFO("turning right");
    ros::Publisher cmd_vel_pub = ros::NodeHandle().advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    // 设置发布频率
    ros::Rate rate(10); // 10Hz

    // 创建Twist消息实例
    geometry_msgs::Twist twist;

    // 设置旋转速度 (绕z轴)
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    twist.angular.z = -2.0; // 旋转速度为-1弧度/秒

    ROS_INFO("Rotating the robot");

    for(int count=9;count>0;count--) 
    {
        // 发布消息
        cmd_vel_pub.publish(twist);
        // 休眠直到下一个发布周期
        rate.sleep();
    }
}

void turn_right_45(void) 
{
    // 使用全局命名空间创建Publisher，不需要显式创建节点句柄
    ROS_INFO("turning right");
    ros::Publisher cmd_vel_pub = ros::NodeHandle().advertise<geometry_msgs::Twist>("/cmd_vel", 10);

    // 设置发布频率
    ros::Rate rate(10); // 10Hz

    // 创建Twist消息实例
    geometry_msgs::Twist twist;

    // 设置旋转速度 (绕z轴)
    twist.linear.x = 0.0;
    twist.linear.y = 0.0;
    twist.linear.z = 0.0;
    twist.angular.x = 0.0;
    twist.angular.y = 0.0;
    //twist.angular.z = -2.0; // 旋转速度为-1弧度/秒

    ROS_INFO("Rotating the robot");

    for(int count=7;count>0;count--) 
    {
        // 发布消息
        cmd_vel_pub.publish(twist);
        // 休眠直到下一个发布周期
        rate.sleep();
    }
}

bool getRobotPosition(tf::TransformListener& listener, double& x, double& y, double& yaw) {
    tf::StampedTransform transform;
    try {
        // 获取/map到/base_link的变换关系，时间为当前最新
        listener.lookupTransform("map", "base_link", ros::Time(0), transform);

        // 获取位置坐标
        x = transform.getOrigin().x();
        y = transform.getOrigin().y();

        // 获取四元数
        tf::Quaternion q = transform.getRotation();

        // 计算Yaw角
        yaw = tf::getYaw(q);

        return true; // 成功获取坐标和Yaw角
    } catch (tf::TransformException &ex) {
        ROS_WARN("Failed to get robot position: %s", ex.what());
        return false; // 获取坐标失败
    }
}


void sendGoals(const vector<Goal>& goals, MoveBaseClient& ac) 
{
    tf::TransformListener listener;
    for (size_t i = 0; i <goals.size(); i++) 
    {

        move_base_msgs::MoveBaseGoal goal;
        goal.target_pose.header.frame_id = "map";
        goal.target_pose.header.stamp = ros::Time::now();
        goal.target_pose.pose.position.x = goals[i].x;
        goal.target_pose.pose.position.y = goals[i].y;
        goal.target_pose.pose.orientation.z = goals[i].orientation_z;
        goal.target_pose.pose.orientation.w = goals[i].orientation_w;

        ac.sendGoal(goal);
        ROS_INFO("Sending Goal %zu", i +1);

        ac.waitForResult();
        if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            sleep(1);
            ROS_INFO("Goal %zu achieved success!", i + 1);

            switch (i)
            {
                case 0:
                    break;
                case 1:
                    break;
                case 2:
                    break;
                case 3:
                    break;
                case 4:
                   break;
                case 5: 
                    break;
                default:
                    break;
            }
        } 
        else 
        {
            ROS_WARN("Failed to achieve Goal %zu", i + 1);
        }
    }
}

void robotNavigation(int argc,char** argv,const string& goals_file, const string& launch_file) 
{
    ros::init(argc,argv,"send_goals");
    // ros::NodeHandle nh;
    
    MoveBaseClient ac("move_base", true);
    cout << "启动导航"<<endl;
    
    while (!ac.waitForServer(ros::Duration(5.0))) {
        ROS_INFO("Waiting for the move_base action server to come up");
    }

    vector<Goal> goals = readGoalsFromFile(goals_file);
    if (goals.empty()) {
        ROS_ERROR("Failed to read goals from file: %s", goals_file.c_str());
        return;
    }

    sendGoals(goals, ac);
}


int main(int argc,char*argv[])
{

    ros::init(argc, argv, "main");

    // 获取开始时间（高精度）
    ros::WallTime start = ros::WallTime::now();
    ros::NodeHandle nh;

    // 指定目标点文件的绝对路径
    string goals_file = "/home/bobac3/ros_workspace/src/send_goals/goals/goals.txt"; // 路径改成自己的
    // 指定启动底层文件的绝对路径
    string launch_file = "/home/bobac3/ros_workspace/src/bobac3_navigation_1/launch/demo_nav_2d.launch"; // 改成自己的，或者去掉，手动启动底层

    thread navigationThread(robotNavigation, argc, argv, goals_file, launch_file);
    navigationThread.join();

    // 获取结束时间
    ros::WallTime end = ros::WallTime::now();

    // 计算时间差
    ros::WallDuration duration = end - start;

    ROS_WARN("实际耗时: %.6f 秒", duration.toSec());
    return 0;
}
